/*
 * Leg.cpp
 *
 *  Created on: Feb 19, 2011
 *      Author: Anton
 */

#include <motion/Leg.h>

Leg::Leg(){
	//cout << "Leg" << endl;
	links = new Link[LINKS];
	unit=0;
	x=0;
	y=0;
	z=0;
	desiredx=0;
	desiredy=0;
	desiredz=0;
}

Leg::~Leg() {
	//cout << "\t\tDestroy Links" << endl;
	delete[] links;
	links = NULL;
	//cout << "Links Destroyed" << endl;
}

double Leg::getUnit() const
{
    return unit;
}

double Leg::getX() const
{
    return x;
}

double Leg::getY() const
{
    return y;
}

double Leg::getZ() const
{
    return z;
}

void Leg::setUnit(double unit)
{
    this->unit = unit;
}

void Leg::setX(double x)
{
    this->x = x;
}

void Leg::setY(double y)
{
    this->y = y;
}

void Leg::setZ(double z)
{
    this->z = z;
}

double Leg::getDesiredx() const
{
    return desiredx;
}

double Leg::getDesiredy() const
{
    return desiredy;
}

double Leg::getDesiredz() const
{
    return desiredz;
}

void Leg::setDesiredx(double desiredx)
{
    this->desiredx = desiredx;
}

void Leg::setDesiredy(double desiredy)
{
    this->desiredy = desiredy;
}

void Leg::setDesiredz(double desiredz)
{
    this->desiredz = desiredz;
}

Matrix Leg::forwardK(){
	Matrix FK = Matrix(4,4, 1.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0, 0.0,0.0,1.0,0.0, 0.0,0.0,0.0,1.0);
	for(int i=0; i<LINKS; i++){
		Matrix B = Leg::links[i].forwardK();
		FK = FK*B;
	}
	//cout << "LEG: " << FK << endl;
	return FK;
}

Matrix Leg::inverseK(){
	double x = Leg::getDesiredx();
	double y = Leg::getDesiredy();
	double z = Leg::getDesiredz();
	double theta1 = Leg::links[0].getDH().getvalue(0,3);
	double theta2 = Leg::links[1].getDH().getvalue(0,3);
	double theta3 = Leg::links[2].getDH().getvalue(0,3);

	double theta1ik = atan2(z,x)*RAD2DEG-theta1-180;
	double r = sqrt(pow(x,2)+pow(z,2))-LINK1LENGTH;
	double l = pow(r,2) + pow(y,2);
	double d = (l-pow(LINK2LENGTH,2)-pow(LINK3LENGTH,3))/(2*LINK2LENGTH*LINK3LENGTH);

	double theta3ik=atan2(Leg::getUnit()*sqrt(1-pow(d,2)),d)*RAD2DEG;
	double theta2ik=atan2(y,r)*RAD2DEG-atan2(LINK3LENGTH*sin(theta3ik*DEG2RAD),LINK2LENGTH+LINK3LENGTH*cos(theta3ik*DEG2RAD))*RAD2DEG;

	theta1ik = theta1ik-90;
	theta2ik = -theta2ik-theta2;
	theta3ik = -theta3ik-theta3;

	//In here write a function to convert this to raw data!!!
	Matrix m = Matrix(1,3,theta1ik,theta2ik,theta3ik);
	return m;
}
